/*
 * This command searches for a target using the Axis Camera.
 */

package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;

/**
 *
 * @author Jarrod Risley
 * 
 * This command is called when the arm operator presses the assigned button
 * and tracks a laser pointer's pointer. The robot will only - AND ONLY (for 
 * now) - print out the location of a target in the command line. NO OTHER 
 * ACTIONS WILL TAKE PLACE UNTIL A SOLID TARGETING ROUTINE HAS BEEN CREATED.
 * 
 * That means, "don't go off and make the robot try to move with the target 
 * before we figure out how this is all going to work."
 */



public class SearchForTargets extends CommandBase {

    /*
     * Global Declarations
     */
    private ColorImage colorIm;
    private BinaryImage bi;
    
    private boolean isFinished = false;
    
    public SearchForTargets() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        
        // Will eventually require the drive train and several other components.
        requires(AxisCamera);
        
        if(!AxisCamera.getCameraState()) {
            isFinished = true;
            System.out.println("Camera Subsystem offline. Unable to track "
                    + "targets.");
        } // end if
        
    } // end Constructor

    // Called just before this Command runs the first time
    protected void initialize() {
        
        colorIm = AxisCamera.getImage();
        
    } // end initialize

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        
        bi = AxisCamera.filterParticles(AxisCamera.thresholdImage(colorIm));
        
        AxisCamera.analyze(bi);
        
        isFinished = true;
        
    } // end execute

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return isFinished;
    } // end isFinished

    // Called once after isFinished returns true
    protected void end() {
        
        // Make sure that these are actually deleted so we don't bury the cRio.
        AxisCamera.deleteImage(bi);
        AxisCamera.deleteImage(colorIm);
        
    } // end end....lolz

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        
        System.out.println("SearchForTargets has been interrupted.");
        end();
        
    } // end interrupted
    
} // end SearchForTargets